from enum import Enum
from typing import List, Tuple
from common.ReferenceLine import ReferenceLine
from protoclass.PathPoint import PathPoint
from protoclass.FrenetFramePoint import FrenetFramePoint
from common.DiscretizedPath import DiscretizedPath
from common.FrenetFramePath import FrenetFramePath
from logging import Logger
from copy import deepcopy
from protoclass.SLBoundary import SLPoint
from common.ReferencePoint import ReferencePoint
from CartesianFrenetConverter import CartesianFrenetConverter
from common.Vec2d import Vec2d
from config import FLAGS_trajectory_point_num_for_debug

logger = Logger("PathData")

class PathPointType(Enum):

    IN_LANE = 0
    OUT_ON_FORWARD_LANE = 1
    OUT_ON_REVERSE_LANE = 2
    OFF_ROAD = 3
    UNKNOWN = 4

class PathData:
    """
    PathData class
    """

    def __init__(self):
        """
        Constructor
        """

        self._reference_line: ReferenceLine = None
        self._discretized_path: DiscretizedPath = DiscretizedPath()
        self._frenet_path: FrenetFramePath = FrenetFramePath()

        self._path_point_decision_guide: List[Tuple[float, PathPointType, float]] = []
        """
        speed decision generated by path analyzer for guiding speed limit
        generation in speed bounds decider
        tuple consists of s axis position on reference line; PathPointType
        Enum; distance to closest obstacle
        """
        self._path_label: str = ""
        self._blocking_obstacle_id: str = ""

        self._is_valid_path_reference: bool = False
        """
        parameters for using the learning model output as a path reference
        wheter this PathData is a path reference serving as an optimization target
        for later modules
        """
        self._is_optimized_towards_trajectory_reference = False
        """
        Given a trajectory reference, whether this PathData is optimized
        according to the "path" part of the trajectory so that "speed" part of the
        trajectory could be used in later modules accordingly
        """
        self._is_reverse_path = False
        self._path_reference: List[PathPoint] = []
        """path reference"""

    def SetDiscretizedPath(self, path: DiscretizedPath) -> bool:
        """
        Set the discretized path

        :param DiscretizedPath path: The discretized path
        :returns: True if successful, False otherwise
        :rtype: bool
        """

        if self._reference_line is None:
            logger.error(f"Should NOT set discretized path when reference line is null. Please set reference line first.")
            return False
        self._discretized_path = path
        tag, self._frenet_path = self.XYToSL(self._discretized_path)
        if not tag:
            logger.error(f"Fail to transfer discretized path to frenet path")
            return False
        assert len(self._discretized_path) == len(self._frenet_path)
        return True

    def SetFrenetPath(self, frenet_path: FrenetFramePath) -> bool:
        """
        Set the frenet path

        :param FrenetFramePath frenet_path: The frenet path
        :returns: True if successful, False otherwise
        :rtype: bool
        """
        
        if self._reference_line is None:
            logger.error(f"Should NOT set frenet path when reference line is null. Please set reference line first.")
            return False
        self._frenet_path = frenet_path
        tag, self._discretized_path = self.SLToXY(self._frenet_path)
        if not tag:
            logger.error(f"Fail to transfer frenet path to discretized path")
            return False
        assert len(self._discretized_path) == len(self._frenet_path)
        return True

    def SetReferenceLine(self, reference_line: ReferenceLine) -> None:
        """
        Set the reference line

        :param ReferenceLine reference_line: The reference line
        """

        self.Clear()
        self._reference_line = reference_line

    def SetPathPointDecisionGuide(self, path_point_decision_guide: List[Tuple[float, PathPointType, float]]) -> bool:
        """
        Set the path point decision guide

        :param List[Tuple[float, PathPointType, float]] path_point_decision_guide: The path point decision guide
        :returns: True if successful, False otherwise
        :rtype: bool
        """
    
        if self._reference_line is None:
            logger.error(f"Should NOT set path_point_decision_guide when reference line is null.")
            return False
        if len(self._frenet_path) == 0 or len(self._discretized_path) == 0:
            logger.error(f"Should NOT set path_point_decision_guide when frenet_path or world frame trajectory is empty.")
            return False
        self._path_point_decision_guide = path_point_decision_guide
        return True

    @property
    def discretized_path(self) -> DiscretizedPath:
        """
        Get the discretized path

        :returns: The discretized path
        :rtype: DiscretizedPath
        """

        return self._discretized_path

    @property
    def frenet_frame_path(self) -> FrenetFramePath:
        """
        Get the frenet path

        :returns: The frenet path
        :rtype: FrenetFramePath
        """

        return self._frenet_path

    @property
    def path_point_decision_guide(self) -> List[Tuple[float, PathPointType, float]]:
        """
        Get the path point decision guide

        :returns: The path point decision guide
        :rtype: List[Tuple[float, PathPointType, float]]
        """

        return self._path_point_decision_guide

    def GetPathPointWithPathS(self, s: float) -> PathPoint:
        """
        Get the path point with the given s value

        :param float s: The s value
        """

        return self._discretized_path.Evaluate(s)

    def GetPathPointWithRefS(self, ref_s: float) -> Tuple[bool, PathPoint]:
        """
        this function will find the path_point in discretized_path whose
        projection to reference line has s value closest to ref_s.

        :param float ref_s: The reference s value
        :returns: a tuple of success flag and the path point
        :rtype: Tuple[bool, PathPoint]
        """

        assert self._reference_line is not None, "Reference line is not set"
        assert len(self._discretized_path) == len(self._frenet_path), "Discretized path and frenet path have different length"
        if ref_s < 0:
            logger.error(f"ref_s [{ref_s}] should be > 0")
            return False, None
        if ref_s > self._frenet_path[-1].s:
            logger.error(f"ref_s is larger than the length of self._frenet_path length [{self._frenet_path[-1].s}].")
            return False, None
        
        index: int = 0
        kDistanceEpsilon: float = 1e-3
        for i in range(len(self._frenet_path) - 1):
            if abs(ref_s - self._frenet_path[i].s) < kDistanceEpsilon:
                path_point = deepcopy(self._discretized_path[i])
                return True, path_point
            if self._frenet_path[i].s < ref_s <= self._frenet_path[i + 1].s:
                index = i
                break
        r: float = (ref_s - self._frenet_path[index].s) / (self._frenet_path[index + 1].s - self._frenet_path[index].s)

        discretized_path_s: float = self._discretized_path[index].s + r * (self._discretized_path[index + 1].s - self._discretized_path[index].s)
        path_point = deepcopy(self._discretized_path.Evaluate(discretized_path_s))

        return True, path_point

    def LeftTrimWithRefS(self, frenet_point: FrenetFramePoint) -> bool:
        """
        Left trim with ref_s

        :param FrenetFramePoint frenet_point: The frenet point
        :returns: True if successful, False otherwise
        :rtype: bool
        """

        assert self._reference_line is not None, "Reference line is not set"
        frenet_frame_points: List[FrenetFramePoint] = []
        frenet_frame_points.append(frenet_point)

        for fp in self._frenet_path:
            if abs(fp.s - frenet_point.s) < 1e-6:
                continue
            if fp.s > frenet_point.s:
                frenet_frame_points.append(fp)
        self.SetFrenetPath(FrenetFramePath(frenet_frame_points))
        return True

    def UpdateFrenetFramePath(self, reference_line: ReferenceLine) -> bool:
        """
        Update the frenet frame path

        :param ReferenceLine reference_line: The reference line
        :returns: True if successful, False otherwise
        :rtype: bool
        """

        self._reference_line = reference_line
        return self.SetDiscretizedPath(self._discretized_path)

    def Clear(self) -> None:
        """
        Clear method
        """

        self._discretized_path.clear()
        self._frenet_path.clear()
        self._path_point_decision_guide.clear()
        self._path_reference.clear()
        self._reference_line = None

    def Empty(self) -> bool:
        """
        Judge if the path is empty
        """

        return len(self._discretized_path) == 0 and len(self._frenet_path) == 0

    def __str__(self) -> str:
        """
        String representation of the PathData object

        :returns: The string representation of the PathData object
        :rtype: str
        """

        limit: int = min(len(self._discretized_path), FLAGS_trajectory_point_num_for_debug)
        points_str = ",\n".join(str(point) for point in self._discretized_path[:limit])
        return f"[\n{points_str}\n]"

    def set_path_label(self, label: str) -> None:
        """
        Set the path label

        :param str label: The path label
        """

        self._path_label = label

    @property
    def path_label(self) -> str:
        """
        Get the path label

        :returns: The path label
        :rtype: str
        """

        return self._path_label

    def set_blocking_obstacle_id(self, obs_id: str) -> None:
        """
        Set the blocking obstacle id

        :param str obs_id: The blocking obstacle id
        """

        self._blocking_obstacle_id = obs_id

    def blocking_obstacle_id(self) -> str:
        """
        Get the blocking obstacle id

        :returns: The blocking obstacle id
        :rtype: str
        """

        return self._blocking_obstacle_id

    def is_valid_path_reference(self) -> bool:
        """
        Check if the path reference is valid

        :returns: True if the path reference is valid, False otherwise
        :rtype: bool
        """

        return self._is_valid_path_reference

    def set_is_valid_path_reference(self, is_valid_path_reference: bool) -> None:
        """
        Set the is valid path reference flag

        :param bool is_valid_path_reference: The is valid path reference flag to set
        """

        self._is_valid_path_reference = is_valid_path_reference

    def is_optimized_towards_trajectory_reference(self) -> bool:
        """
        Check if the path is optimized towards the trajectory reference

        :returns: True if the path is optimized towards the trajectory reference, False otherwise
        :rtype: bool
        """

        return self._is_optimized_towards_trajectory_reference

    def set_is_optimized_towards_trajectory_reference(self, is_optimized_towards_trajectory_reference: bool) -> None:
        """
        Set the is optimized towards trajectory reference flag

        :param bool is_optimized_towards_trajectory_reference: The is optimized towards trajectory reference flag to set
        """

        self._is_optimized_towards_trajectory_reference = is_optimized_towards_trajectory_reference

    @property
    def path_reference(self) -> List[PathPoint]:
        """
        Get the path reference

        :returns: The path reference
        :rtype: List[PathPoint]
        """

        return self._path_reference

    def set_path_reference(self, path_reference: List[PathPoint]) -> None:
        """
        Set the path reference

        :param List[PathPoint] path_reference: The path reference
        """

        self._path_reference = path_reference

    def is_reverse_path(self) -> bool:
        """
        Check if the path is a reverse path

        :returns: True if the path is a reverse path, False otherwise
        :rtype: bool
        """

        return self._is_reverse_path

    def set_is_reverse_path(self, is_reverse_path: bool) -> None:
        """
        Set the is reverse path flag

        :param bool is_reverse_path: The is reverse path flag to set
        """

        self._is_reverse_path = is_reverse_path

    def SLToXY(self, frenet_path: FrenetFramePath) -> Tuple[bool, DiscretizedPath]:
        """
        convert frenet path to cartesian path by reference line

        :param FrenetFramePath frenet_path: The frenet path
        :returns: A tuple of success flag and the discretized path
        :rtype: Tuple[bool, DiscretizedPath]
        """

        path_points: List[PathPoint] = []
        for frenet_point in frenet_path:
            sl_point: SLPoint = SLPoint(frenet_point.s, frenet_point.l)
            tag, cartesian_point = self._reference_line.SLToXY(sl_point)
            if not tag:
                logger.error(f"Fail to convert sl point to xy point")
                return False, DiscretizedPath()
            ref_point: ReferencePoint = self._reference_line.GetReferencePoint(frenet_point.s)
            theta: float = CartesianFrenetConverter.CalculateTheta(ref_point.heading, ref_point.kappa, frenet_point.l, frenet_point.dl)
            logger.debug(f"frenet_point: {frenet_point}")
            kappa: float = CartesianFrenetConverter.CalculateKappa(ref_point.kappa, ref_point.dkappa, frenet_point.l, frenet_point.dl, frenet_point.ddl)

            s: float = 0.0
            dkappa: float = 0.0
            if len(path_points) > 0:
                last: Vec2d = Vec2d(path_points[-1].x, path_points[-1].y)
                distance: float = (last - cartesian_point).Length()
                s = path_points[-1].s + distance
                dkappa = (kappa - path_points[-1].kappa) / distance
            path_points.append(PathPoint(cartesian_point.x, cartesian_point.y, 0.0, theta, kappa, s, dkappa))

        return True, DiscretizedPath(path_points)

    def XYToSL(self, discretized_path: DiscretizedPath) -> Tuple[bool, FrenetFramePath]:
        """
        convert cartesian path to frenet path by reference line

        :param DiscretizedPath discretized_path: The discretized path
        :returns: A tuple of success flag and the frenet path
        :rtype: Tuple[bool, FrenetFramePath]
        """

        assert self._reference_line is not None, "Reference line is not set"
        frenet_frame_points: List[FrenetFramePoint] = []
        max_len: float = self._reference_line.Length()
        for path_point in discretized_path:
            frenet_point: FrenetFramePoint = self._reference_line.GetFrenetPoint(path_point)
            if frenet_point.s is None:
                tag, sl_point = self._reference_line.XYToSL(path_point)
                if not tag:
                    logger.error(f"Fail to transfer cartesian point to frenet point.")
                    return False, FrenetFramePath()
                # NOTICE: does not set dl and ddl here. Add if needed.
                frenet_point = FrenetFramePoint(s=max(0.0, min(sl_point.s, max_len)),
                                                l=sl_point.l)
                frenet_frame_points.append(frenet_point)
                continue
            frenet_point.s = max(0.0, min(frenet_point.s, max_len))
            frenet_frame_points.append(frenet_point)
        
        return True, FrenetFramePath(frenet_frame_points)
